// 头文件
#include <Servo.h>    // 舵机头文件
#include <U8g2lib.h>  // OLED显示屏头文件

#define canMinSpeed 180            // 最小驱动的PWM值
#define minUltrasonicDistance 20   // 超声波测距前进距离的判定限度
#define minCanSpeed 180            // 最小驱动的PWM值
#define trackingSpeed 180          // 循迹速度
#define searchLineSpeed 180        // 循迹速度
#define servoTransAnglePerTime 10  // 舵机每次转动的角度

// OLED
U8G2_SSD1306_128X64_NONAME_F_SW_I2C oled(U8G2_R0, SCL, SDA, U8X8_PIN_NONE);  // 软件I2C通信，可显示多行文字

// 函数声明
void getBluetoothStringMesage();  // 获得蓝牙输入

void ultrasonicDistance();  // 获得超声波距离

// 电机控制的引脚
/*
*电机控制的真值表:
*|电机转动效果   | IN1     IN2    | IN3     IN4|
*|左边电机正转   |  1       0     |  -       - |
*|左边电机反转   |  0       1     |  -       - |
*|左边电机停止   |  0       0     |  -       - |
*|右边电机正转   |  -       -     |  1       0 |
*|右边电机反转   |  -       -     |  0       1 |
*|右边电机停止   |  -       -     |  0       0 |
*/
const int motorControlLeftIN1 = 7;  // 左边电机控制的IN脚
const int motorControlLeftIN2 = 8;
const int motorControlRightIN1 = 9;  // 右边电机控制的IN脚
const int motorControlRightIN2 = 10;
const int motorControlLeftPWM = 6;    // 控制左边电机速度的pwm引脚
const int motorControlRightPWM = 11;  // 控制右边电机速度的pwm引脚

// 循迹模块的引脚
const int leftTrackModelAnalogPin = A0;    // 左边循迹模块
const int middleTrackModelAnalogPin = A1;  // 中间循迹模块
const int rightTrackModelAnalogPin = A2;   // 右边循迹模块

// 红外模块的引脚
const int leftRadioModelOut = 13;   // 左边红外模块
const int rightRadioModelOut = 12;  // 右边红外模块

// 超声波模块的引脚
const int ultrasonicTrigPin = 3;  // trig
const int ultrasonicEchoPin = 2;  // echo

const int servoModelPin = 5;  // 舵机模块的引脚
const int buzzer = 4;         // 蜂鸣器的引脚
const int warningLed = A3;    // 警告灯

Servo servoControl;           // 初始化舵机
String bluetoothMessageInfo;  // 从蓝牙得到的控制信息

int canonSpeedVar = 180;      // 电机转速
int ultrasonic_distance = 0;  // 超声波测距所得距离

bool autoTrackingFlag = 0;      // 是否开启自动循迹的标志位
bool searchLineFlag = 0;        // 是否开启扫线的标志位
bool bluetoothControlFlag = 0;  // 是否开启蓝牙控制

// 空旷场地的判定计数
int wideGroundJudeCount = 0;  // 大于10次且红外传感器没亮就认为是空旷场地

// 巡线模块电压输出
int leftVal = 0;   // 左
int rightVal = 0;  // 右

// 是否刷新屏幕
bool trackingRefresh = 0;   // 避障
bool lineRefresh = 0;       // 循迹
bool bluetoothRefresh = 0;  // 蓝牙
bool normalRefrsh = 0;      // 正常状态
bool outOfRacing = 0;       // 冲出赛道
bool groundError = 0;       // 死角

// 获得蓝牙输入
void getBluetoothStringMesage() {
  if (!Serial.available())
    return;
  String str = "";
  while (Serial.available()) {
    char tmp = Serial.read();
    str += tmp;
  }
  if (str != "") {
    bluetoothMessageInfo = str;
  }
}

// 获得超声波测距的距离
void ultrasonicDistance() {
  // 获得距离
  digitalWrite(ultrasonicTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(ultrasonicTrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(ultrasonicTrigPin, LOW);
  ultrasonic_distance = pulseIn(ultrasonicEchoPin, HIGH) / 58;
  delay(50);  // 需要延时一段事件以减少回音的影响
}

// 左边电机状态:1,前进，0，后退，默认前进
void controlLeftMOtorType(bool flag = true) {
  if (flag) {
    digitalWrite(motorControlLeftIN1, HIGH);
    digitalWrite(motorControlLeftIN2, LOW);
  } else {
    digitalWrite(motorControlLeftIN1, LOW);
    digitalWrite(motorControlLeftIN2, HIGH);
  }
}

// 右边电机状态:1,前进，0，后退，默认前进
void controlRightMotorType(bool flag = true) {
  if (flag) {
    digitalWrite(motorControlRightIN1, HIGH);
    digitalWrite(motorControlRightIN2, LOW);
  } else {
    digitalWrite(motorControlRightIN1, LOW);
    digitalWrite(motorControlRightIN2, HIGH);
  }
}

// 左边电机pwm调速
void leftMotorPwm(int pwmValue) {
  analogWrite(motorControlLeftPWM, pwmValue);
}

// 右边电机pwm调速
void rightMotorPwm(int pwmValue) {
  analogWrite(motorControlRightPWM, pwmValue);
}

// pwm限幅
int limPwm(int value) {
  int a = value;
  if (a <= 0)
    a = 0;
  if (a >= 255)
    a = 255;
  return a;
}

// 电机控制
void canonControlDirction(bool left_flag = true, bool right_flag = true, int left_motor_pwm = 130, int right_motor_pwm = 135) {
  // 控制方向
  controlLeftMOtorType(left_flag);
  controlRightMotorType(right_flag);
  // 设置pwm
  leftMotorPwm(limPwm(left_motor_pwm));
  rightMotorPwm(limPwm(right_motor_pwm));
}

// 刹车
void brakeCar() {
  digitalWrite(motorControlLeftIN1, LOW);
  digitalWrite(motorControlLeftIN2, LOW);
  digitalWrite(motorControlRightIN1, LOW);
  digitalWrite(motorControlRightIN2, LOW);
  analogWrite(motorControlLeftPWM, 0);
  analogWrite(motorControlRightPWM, 0);
}

// 警告信息
void warningInfo() {
  pinMode(buzzer, OUTPUT);
  tone(buzzer, 500);             // 警告信息，蜂鸣器响
  analogWrite(warningLed, 255);  // 警告信息，红灯亮起
}

// 恢复至正常状态
void resetType() {
  noTone(buzzer);              // 关闭蜂鸣器
  pinMode(buzzer, INPUT);      // 打断施法，直接将引脚设定为输入模式
  analogWrite(warningLed, 0);  // 关闭红灯
}

// 自动寻线
void autoSearchLine() {
  // 获取信号, LOW为没有黑线，HIGH为有黑线
  leftVal = digitalRead(leftTrackModelAnalogPin);    // 左
  rightVal = digitalRead(rightTrackModelAnalogPin);  // 右
  // 中间一个为1，直接直行
  if (leftVal == 1 && rightVal == 1) {
    canonControlDirction(true, true, 255, 255);  // 前进
  } else if (leftVal == 1 && rightVal == 0) {
    canonControlDirction(false, true, 255, 255);  // 左转
  } else if ((leftVal == 0) && rightVal == 1) {
    canonControlDirction(true, false, 255, 255);  // 右转
  } else if (rightVal == 0 && leftVal == 0) {
    if (outOfRacing == 0) {
      oled.clear();
      oled.setCursor(0, 15);
      oled.print("Out of road.");
      oled.sendBuffer();
      outOfRacing = 1;  // 避免再次运行
    }
    // 跑出赛道，需要闪烁红灯，闪烁两下之后持续亮灯
    analogWrite(warningLed, 255);
    delay(500);
    analogWrite(warningLed, 0);
    delay(500);
    analogWrite(warningLed, 255);
    delay(500);
    analogWrite(warningLed, 0);
    delay(500);
    analogWrite(warningLed, 255);
    while (1) {
      leftVal = digitalRead(leftTrackModelAnalogPin);    // 左
      rightVal = digitalRead(rightTrackModelAnalogPin);  // 右
      if (leftVal == 1 || rightVal == 1) {
        break;
      }
      canonControlDirction(false, false, 255, 255);
      delay(20);
      brakeCar();
    }
    brakeCar();
    analogWrite(warningLed, 0);  // 回到赛道，停止亮灯
    lineRefresh = 1;             // 刷新一次屏幕
    outOfRacing = 0;             // 重置标志位
  }
}

// 避障
void autoTracking() {
  if (ultrasonic_distance >= minUltrasonicDistance) {
    // 出现错误的距离
    if (ultrasonic_distance > 400) {
      resetType();
      brakeCar();
      warningInfo();
      delay(500);
      resetType();
      delay(100);
      warningInfo();
      delay(500);
      resetType();
      delay(100);
      warningInfo();
      delay(500);
      resetType();
      delay(2000);
      int leftRadioVal = digitalRead(leftRadioModelOut);    // 左
      int rightRadioVal = digitalRead(rightRadioModelOut);  // 右
      if (leftRadioVal == 1 || rightRadioVal == 1) {
        wideGroundJudeCount += 1;
      }
      if (wideGroundJudeCount >= 3) {
        resetType();                                 // 复位
        canonControlDirction(true, true, 120, 120);  // 距离满足安全距离，前进
        groundError = 0;                             // 重置标志位
        return;
      } else {
        return;
      }
      return;
    }
    resetType();                                 // 复位
    canonControlDirction(true, true, 120, 120);  // 距离满足安全距离，前进
    groundError = 0;                             // 重置标志位
    wideGroundJudeCount = 0;                     // 重置空旷场地计数判定
  } else {
    // 出现错误的距离
    if (ultrasonic_distance < 2) {
      resetType();
      brakeCar();
      warningInfo();
      delay(500);
      resetType();
      delay(100);
      warningInfo();
      delay(500);
      resetType();
      delay(2000);
      int leftRadioVal = digitalRead(leftRadioModelOut);    // 左
      int rightRadioVal = digitalRead(rightRadioModelOut);  // 右
      if (leftRadioVal == 1 || rightRadioVal == 1) {
        wideGroundJudeCount += 1;
      }
      if (wideGroundJudeCount >= 10) {
        canonControlDirction(true, true, 120, 120);  // 距离满足安全距离，前进
        return;
      } else {
        return;
      }
      return;
    }
    // 去除掉特殊情况
    int leftRadioVal = digitalRead(leftRadioModelOut);    // 左
    int rightRadioVal = digitalRead(rightRadioModelOut);  // 右
    if (leftRadioVal == 0 && rightRadioVal == 0) {
      resetType();
      brakeCar();  // 刹车
      return;
    } else if (leftRadioVal == 1 && rightRadioVal == 0) {
      resetType();
      brakeCar();  // 刹车
      // 稍微向左转动一点距离
      canonControlDirction(false, true, 255, 255);
      delay(10);
      brakeCar();
      return;
    } else if (leftRadioVal == 0 && rightRadioVal == 1) {
      resetType();
      brakeCar();  // 刹车
      // 稍微向右转动一点距离
      canonControlDirction(true, false, 255, 255);
      delay(10);
      brakeCar();
      return;
    }
    wideGroundJudeCount = 0;  // 重置空旷场地计数判定
    brakeCar();               // 距离不满足安全距离，刹车
    warningInfo();            // 显示告警
    servoControl.write(180);
    delay(500);
    ultrasonicDistance();
    // 获得有效距离
    while (ultrasonic_distance < 2 || ultrasonic_distance > 400)
      ultrasonicDistance();
    int leftDirctDistance = ultrasonic_distance;
    servoControl.write(0);
    delay(500);
    ultrasonicDistance();
    // 获得有效距离
    while (ultrasonic_distance < 2 || ultrasonic_distance > 400)
      ultrasonicDistance();
    int rightDirctDistance = ultrasonic_distance;
    servoControl.write(90);
    delay(50);
    resetType();  // 复位
    // 左右两侧距离都大于最大上限
    if (rightDirctDistance >= minUltrasonicDistance && leftDirctDistance >= minUltrasonicDistance) {
      // 向后走，直到走到范围之外
      canonControlDirction(false, false, 255, 255);
      delay(200);
      brakeCar();  // 刹车
      canonControlDirction(false, true, 255, 255);
      delay(1000);
      brakeCar();
      groundError = 0;  // 重置标志位
    } else if (leftDirctDistance >= minUltrasonicDistance && rightDirctDistance < minUltrasonicDistance) {
      canonControlDirction(false, true, 255, 255);
      delay(1000);
      brakeCar();
      groundError = 0;  // 重置标志位
    } else if (leftDirctDistance < minUltrasonicDistance && rightDirctDistance >= minUltrasonicDistance) {
      canonControlDirction(true, false, 255, 255);
      delay(1200);
      brakeCar();
      groundError = 0;  // 重置标志位
    } else if (leftDirctDistance < minUltrasonicDistance && rightDirctDistance < minUltrasonicDistance) {
      if (leftDirctDistance < 10 || rightDirctDistance < 10) {
        if (groundError == 0) {
          oled.clear();
          oled.setCursor(0, 15);
          oled.print("Road error");
          oled.sendBuffer();
          groundError = 1;
        }
        // 有效距离太小，进入死角
        brakeCar();
        resetType();
        brakeCar();
        warningInfo();
        delay(500);
        resetType();
        delay(100);
        warningInfo();
        delay(500);
        resetType();
        warningInfo();
        delay(500);
        resetType();
      } else {
        // 向后走，直到走到范围之外
        canonControlDirction(false, false, 255, 255);
        delay(500);
        brakeCar();
        canonControlDirction(true, false, 255, 255);
        delay(500);
        groundError = 0;  // 重置标志位
      }
    }
  }
}

// 蓝牙控制命令
void bluetoothControl(char command, int MoveSpeed = 180) {
  switch (command) {
    case 'W':
    case 'w':  // 前进不变速
      {
        if (bluetoothControlFlag) {
          canonControlDirction(true, true, canonSpeedVar, canonSpeedVar);  // 前进
          searchLineFlag = 0;
        }
      }
      break;
    case 'S':
    case 's':  // 后退不变速
      {
        if (bluetoothControlFlag) {
          canonControlDirction(false, false, canonSpeedVar, canonSpeedVar);  // 后退
          searchLineFlag = 0;
        }
      }
      break;
    case 'A':
    case 'a':  // 左转
      {
        if (bluetoothControlFlag) {
          canonControlDirction(false, true, canonSpeedVar, canonSpeedVar);
          searchLineFlag = 0;
        }
      }
      break;
    case 'd':  // 右转
    case 'D':
      {
        if (bluetoothControlFlag) {
          canonControlDirction(true, false, canonSpeedVar, canonSpeedVar);
          searchLineFlag = 0;
        }
      }
      break;
    case 'Q':
    case 'q':  // 加速
      {
        canonSpeedVar += 20;
        if (canonSpeedVar >= 255)
          canonSpeedVar = 255;
        if (bluetoothControlFlag) {
          leftMotorPwm(canonSpeedVar);
          rightMotorPwm(canonSpeedVar);
          searchLineFlag = 0;
        }
      }
      break;
    case 'R':
    case 'r':  // 减速
      {
        canonSpeedVar -= 20;
        if (canonSpeedVar <= 0) {
          canonSpeedVar = 0;
        }
        if (bluetoothControlFlag) {
          leftMotorPwm(canonSpeedVar);
          rightMotorPwm(canonSpeedVar);
          searchLineFlag = 0;
        }
      }
      break;
    case 'E':
    case 'e':  // 刹车
      {
        if (bluetoothControlFlag) {
          brakeCar();
          searchLineFlag = 0;
          autoTrackingFlag = 0;
        }
      }
      break;
    case 'p':  // 左漂移，前进2秒，转动2秒
      {
        canonControlDirction(true, true, 255, 255);
        delay(2000);
        canonControlDirction(false, true, 255, 255);
        delay(2000);
        brakeCar();
      }
      break;
    case 'o':  // 右漂移，前进2秒，转动2秒
      {
        canonControlDirction(true, true, 255, 255);
        delay(2000);
        canonControlDirction(true, false, 255, 255);
        delay(2000);
        brakeCar();
      }
      break;
    case 'x':  // 自动循迹开
      {
        searchLineFlag = 1;
        lineRefresh = 1;
      }
      break;
    case 'X':  // 自动循迹关
      {
        searchLineFlag = 0;
        if (bluetoothControlFlag == 0 && searchLineFlag == 0 && autoTrackingFlag == 0)
          normalRefrsh = 1;
      }
      break;
    case 'b':  // 避障开
      {
        searchLineFlag = 0;
        autoTrackingFlag = 1;
        trackingRefresh = 1;
      }
      break;
    case 'B':  // 避障关
      {
        searchLineFlag = 0;
        autoTrackingFlag = 0;
        if (bluetoothControlFlag == 0 && searchLineFlag == 0 && autoTrackingFlag == 0)
          normalRefrsh = 1;
      }
      break;
    case 'k':  // 蓝牙控制开
      {
        searchLineFlag = 0;
        bluetoothControlFlag = 1;
        bluetoothRefresh = 1;
      }
      break;
    case 'K':  // 蓝牙控制关
      {
        searchLineFlag = 0;
        bluetoothControlFlag = 0;
        if (bluetoothControlFlag == 0 && searchLineFlag == 0 && autoTrackingFlag == 0)
          normalRefrsh = 1;
      }
      break;
    case 'l':  // 锁定设备，全部关闭
      {
        brakeCar();
        resetType();
        searchLineFlag = 0;
        bluetoothControlFlag = 0;
        autoTrackingFlag = 0;
        normalRefrsh = 1;
      }
      break;
  }
}

// 初始化引脚
void setup() {
  // 电机，刚开始时电机不动
  // 控制引脚
  pinMode(motorControlLeftIN1, OUTPUT);  // 左边
  digitalWrite(motorControlLeftIN1, LOW);
  pinMode(motorControlLeftIN2, OUTPUT);
  digitalWrite(motorControlLeftIN2, LOW);
  pinMode(motorControlRightIN1, OUTPUT);  // 右边
  analogWrite(motorControlRightIN1, LOW);
  pinMode(motorControlRightIN2, OUTPUT);
  analogWrite(motorControlRightIN2, LOW);
  // 电机pwm
  pinMode(motorControlLeftPWM, OUTPUT);  // 左边
  analogWrite(motorControlLeftPWM, 0);
  pinMode(motorControlRightPWM, OUTPUT);  // 右边
  analogWrite(motorControlRightPWM, 0);

  // 舵机
  servoControl.attach(servoModelPin);

  // 超声波
  pinMode(ultrasonicTrigPin, OUTPUT);
  pinMode(ultrasonicEchoPin, INPUT);

  // 红外
  pinMode(leftRadioModelOut, INPUT);   // 左边
  pinMode(rightRadioModelOut, INPUT);  // 右边

  // 循迹
  pinMode(leftTrackModelAnalogPin, INPUT);   // 左边
  pinMode(rightTrackModelAnalogPin, INPUT);  // 右边

  // 警告灯
  pinMode(warningLed, OUTPUT);
  analogWrite(warningLed, 0);

  // 蓝牙串口
  Serial.begin(9600);

  // OLED显示屏
  oled.setFont(u8g2_font_unifont_t_symbols);  // 设置字体
  oled.begin();                               // 启用OLED显示屏
  oled.clear();
  oled.setCursor(0, 15);
  oled.print("Setup!Ready Go!");
  oled.sendBuffer();

  // 标志位重置
  searchLineFlag = 0;
  bluetoothControlFlag = 0;
  autoTrackingFlag = 0;
  normalRefrsh = 1;

  // 蜂鸣器
  pinMode(buzzer, OUTPUT);
  tone(buzzer, 500);
  delay(1000);
  noTone(buzzer);
  pinMode(buzzer, INPUT);
}

// 循环
void loop() {
  getBluetoothStringMesage();  // 获取蓝牙信息

  // 选择命令
  if (bluetoothMessageInfo != "" && bluetoothMessageInfo) {
    bluetoothControl(bluetoothMessageInfo[0], canonSpeedVar);
    bluetoothMessageInfo = "";
  }

  // 自动循迹
  if (searchLineFlag) {
    if (lineRefresh) {
      oled.clear();
      oled.setCursor(0, 15);
      oled.print("SearchLine...");
      oled.sendBuffer();
      lineRefresh = 0;
      canonSpeedVar = searchLineSpeed;  // 设定电机速度
    }
    autoSearchLine();
  }

  // 自动避障
  if (autoTrackingFlag) {
    if (trackingRefresh) {
      oled.clear();
      oled.setCursor(0, 15);
      oled.print("Tracking...");
      oled.sendBuffer();
      canonSpeedVar = trackingSpeed;  // 设定电机速度
      trackingRefresh = 0;
    }
    ultrasonicDistance();    // 超声波测距
    autoTracking();          // 自动避障
    servoControl.write(90);  // 舵机复位
  }

  // 蓝牙控制
  if (bluetoothControlFlag) {
    if (bluetoothRefresh) {
      oled.clear();
      oled.setCursor(0, 15);
      oled.print("Bluetooth");
      oled.setCursor(0, 30);
      oled.print("Control..");
      oled.sendBuffer();
      bluetoothRefresh = 0;
    }
  }

  if (bluetoothControlFlag == 0 && searchLineFlag == 0 && autoTrackingFlag == 0) {
    if (normalRefrsh) {
      resetType();
      oled.clear();
      oled.setCursor(0, 15);
      oled.print("Mode:None");
      oled.sendBuffer();
      brakeCar();
      normalRefrsh = 0;
    }
  }
}
